#include "pid_ctrl.h"

void pid_ctrl_init(pid_ctrl_t* pid, float kp, float ki, float kd, float dt) 
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->dt = dt;
    pid->target = 0.0f;
    pid->prev_error = 0.0f;
    pid->integral = 0.0f;
    pid->max_integral = 0.2f; // 默认积分限幅
}

void pid_ctrl_set_target(pid_ctrl_t* pid, float target) 
{
    pid->target = target;
}

void pid_ctrl_reset(pid_ctrl_t* pid) 
{
    pid->prev_error = 0.0f;
    pid->integral = 0.0f;
}

float pid_ctrl_update(pid_ctrl_t* pid, float error) 
{    
    // 比例项
    float proportional = pid->kp * error;
    
    // 积分项
    pid->integral += error * pid->dt;
    
    // 积分限幅
    if (pid->integral > pid->max_integral) 
    {
        pid->integral = pid->max_integral;
    } 
    else if (pid->integral < -pid->max_integral) 
    {
        pid->integral = -pid->max_integral;
    }
    
    float integral = pid->ki * pid->integral;
    
    // 微分项
    float derivative = pid->kd * (error - pid->prev_error) / pid->dt;
    
    // 更新上一次误差
    pid->prev_error = error;
    
    // 计算输出
    float output = proportional + integral + derivative;
    
    return output;
}

